%%  Simulation

clear all;close all;clc;

for j=1:6
    xSOF(:,j)=[0.5 -0.5];
    nSOF(j) =norm(xSOF(:,j)); 
end

n=2;m=1;l=1;dM=4;dm=2;
R2=1*eye(m);R1=1*eye(l);
Alpha=1;Beta=1;
JSOF=0;

u1max=1.71;             
y1max=3.9;          

Cu=1;Cy=1;
eps=0.08;r=1;
m1=1+eps;m1=sqrt(m1);
m2=(1+(eps^-1))*(3*r+2);m2=sqrt(m2);
zita=0.8^2;eps1=0.4;eps2=0.4;                                              %New Variables 

% SECOND SYSTEM     Example 1 of MY THIRD PAPER  
el=0.08;
%first sub-system
A10=[1.4 0.2;-0.2 0.2];Ad10=[0 0.01;0.02 0];
B10=[-0.7;1];
E11=0.1*eye(n);F11=0.1*eye(n);G11=[0.1;0];
E11bar=el*E11;F11bar=el*F11;G11bar=el*G11;
F1=[0.01 0;0 0.01];H1=[0.002 0;0 0.002];
%second sub-system
A20=[1.1 0.3;0.5 0.4];Ad20=[0 0.01;0.02 0];
B20=[0.8;-0.5];
C2=[3 0.7];C1=C2;
E12=0.2*eye(n);F12=0.2*eye(n);G12=[0.2;0];
E12bar=el*E12;F12bar=el*F12;G12bar=el*G12;
F2=[0.01 0;0 0.01];H2=[0.002 0;0 0.002];

[U1,C10,V1] = svd(C1);
[U2,C20,V2] = svd(C2);

Vp1=V1(1:n,1:l);Vpp1=V1(1:n,(l+1):(n-l+1));
Vp2=V2(1:n,1:l);Vpp2=V2(1:n,(l+1):(n-l+1));

C10=C10(1:l,1:l);
C20=C20(1:l,1:l);

T0=dM+2;Tf=T0+40;

%***dm=2***
d=[2*ones((Tf-T0)/4,1);3*ones((Tf-T0)/4,1);4*ones((Tf-T0)/4,1);2*ones((Tf-T0)/4,1);4*ones((Tf-T0)/4,1)];

%S1=sdpvar(n,n);S2=sdpvar(n,n);
X11=sdpvar(l,l);X12=sdpvar(l,l);
X21=sdpvar(n-l,n-l);X22=sdpvar(n-l,n-l);
W1=sdpvar(n,n);W2=sdpvar(n,n);
Y1=sdpvar(m,l,'full');Y2=sdpvar(m,l,'full');
Landa1=sdpvar(1,1);Landa2=sdpvar(1,1);
gama=sdpvar(1,1);
G1=sdpvar(m,m);
G2=sdpvar(l,l);

%C=[S1>=0,S2>=0,W1>=0,W2>=0];
C=[X11>=0,X12>=0,X21>=0,X22>=0,W1>=0,W2>=0];
C=C+[Landa1>=0,Landa2>=0];

% OFFLINE LMI for dm=2

%LMI#1      j=1;s=1;
C=C+[[-Vp1*X11*Vp1'-Vpp1*X21*Vpp1' zeros(n) zeros(n,(dM-1)*n) m1*(A10*(Vp1*X11*Vp1'+Vpp1*X21*Vpp1')+B10*Y1*C1)' (Y1*C1)'*R2 Vp1*X11*Vp1'+Vpp1*X21*Vpp1' sqrt(dM-1)*(Vp1*X11*Vp1'+Vpp1*X21*Vpp1') (C1*(Vp1*X11*Vp1'+Vpp1*X21*Vpp1'))'*R1 (m2*E11bar*(Vp1*X11*Vp1'+Vpp1*X21*Vpp1'))' (m2*G11bar*Y1*C1)' (m2*Alpha*F1*(Vp1*X11*Vp1'+Vpp1*X21*Vpp1'))' zeros(n,2*n) zeros(n,(dM-dm)*n);...
    zeros(n) -W1 zeros(n,(dM-1)*n) m1*(Ad10*W1)' zeros(n,m) zeros(n,2*n) zeros(n,l) zeros(n,3*n) (m2*F11bar*W1)' (m2*Beta*H1*W1)' zeros(n,(dM-dm)*n);...
    zeros(n*(dM-dm),2*n) blkdiag(-W2,-W2) zeros(n*(dM-dm),n*(dm-1)) zeros(n*(dM-dm),n) zeros(n*(dM-dm),m) zeros(n*(dM-dm),2*n) zeros(n*(dM-dm),l) zeros(n*(dM-dm),5*n) blkdiag(W2,W2);...
    zeros(n*(dm-1),2*n) zeros(n*(dm-1),n*(dM-dm)) -W2 zeros(n*(dm-1),n) zeros(n*(dm-1),m) zeros(n*(dm-1),2*n) zeros(n*(dm-1),l) zeros(n*(dm-1),5*n) zeros(n*(dm-1),(dM-dm)*n);...
    m1*(A10*(Vp1*X11*Vp1'+Vpp1*X21*Vpp1')+B10*Y1*C1) m1*(Ad10*W1) zeros(n,(dM-1)*n) -Vp1*X11*Vp1'-Vpp1*X21*Vpp1' zeros(n,m) zeros(n,2*n) zeros(n,l) zeros(n,5*n) zeros(n,(dM-dm)*n);...
    R2*(Y1*C1) zeros(m,n) zeros(m,(dM-1)*n) zeros(m,n) -gama*eye(m) zeros(m,2*n) zeros(m,l) zeros(m,5*n) zeros(m,(dM-dm)*n);...
    Vp1*X11*Vp1'+Vpp1*X21*Vpp1' zeros(n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) -W1 zeros(n) zeros(n,l) zeros(n,5*n) zeros(n,(dM-dm)*n);
    sqrt(dM-1)*(Vp1*X11*Vp1'+Vpp1*X21*Vpp1') zeros(n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n) -W2 zeros(n,l) zeros(n,5*n) zeros(n,(dM-dm)*n);...
    R1*(C1*(Vp1*X11*Vp1'+Vpp1*X21*Vpp1')) zeros(l,n) zeros(l,(dM-1)*n) zeros(l,n) zeros(l,m) zeros(l,2*n) -gama*eye(l) zeros(l,5*n) zeros(l,(dM-dm)*n);...
    m2*E11bar*(Vp1*X11*Vp1'+Vpp1*X21*Vpp1') zeros(n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,2*n) zeros(n,l) -Vp1*X11*Vp1'-Vpp1*X21*Vpp1' zeros(n,4*n) zeros(n,(dM-dm)*n);...
    m2*G11bar*Y1*C1 zeros(n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,2*n) zeros(n,l) zeros(n) -Vp1*X11*Vp1'-Vpp1*X21*Vpp1' zeros(n,3*n) zeros(n,(dM-dm)*n);...
    m2*Alpha*F1*(Vp1*X11*Vp1'+Vpp1*X21*Vpp1') zeros(n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,2*n) zeros(n,l) zeros(n,2*n) -Landa1*eye(n) zeros(n,2*n) zeros(n,(dM-dm)*n);...
    zeros(n) m2*F11bar*W1 zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,2*n) zeros(n,l) zeros(n,3*n) -Vp1*X11*Vp1'-Vpp1*X21*Vpp1' zeros(n) zeros(n,(dM-dm)*n);...
    zeros(n) m2*Beta*H1*W1 zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,2*n) zeros(n,l) zeros(n,4*n) -Landa1*eye(n) zeros(n,(dM-dm)*n);...
    zeros((dM-dm)*n,2*n) blkdiag(W2,W2) zeros((dM-dm)*n,(dm-1)*n) zeros((dM-dm)*n,n) zeros((dM-dm)*n,m) zeros((dM-dm)*n,2*n) zeros((dM-dm)*n,l) zeros((dM-dm)*n,5*n) blkdiag(-W1,-W1)]<=0];

%LMI#1      j=1;s=2;
C=C+[[-Vp1*X11*Vp1'-Vpp1*X21*Vpp1' zeros(n) zeros(n,(dM-1)*n) m1*(A10*(Vp1*X11*Vp1'+Vpp1*X21*Vpp1')+B10*Y1*C1)' (Y1*C1)'*R2 Vp1*X11*Vp1'+Vpp1*X21*Vpp1' sqrt(dM-1)*(Vp1*X11*Vp1'+Vpp1*X21*Vpp1') (C1*(Vp1*X11*Vp1'+Vpp1*X21*Vpp1'))'*R1 (m2*E11bar*(Vp1*X11*Vp1'+Vpp1*X21*Vpp1'))' (m2*G11bar*Y1*C1)' (m2*Alpha*F1*(Vp1*X11*Vp1'+Vpp1*X21*Vpp1'))' zeros(n,2*n) zeros(n,(dM-dm)*n);...
    zeros(n) -W1 zeros(n,(dM-1)*n) m1*(Ad10*W1)' zeros(n,m) zeros(n,2*n) zeros(n,l) zeros(n,3*n) (m2*F11bar*W1)' (m2*Beta*H1*W1)' zeros(n,(dM-dm)*n);...
    zeros(n*(dM-dm),2*n) blkdiag(-W2,-W2) zeros(n*(dM-dm),n*(dm-1)) zeros(n*(dM-dm),n) zeros(n*(dM-dm),m) zeros(n*(dM-dm),2*n) zeros(n*(dM-dm),l) zeros(n*(dM-dm),5*n) blkdiag(W2,W2);...
    zeros(n*(dm-1),2*n) zeros(n*(dm-1),n*(dM-dm)) -W2 zeros(n*(dm-1),n) zeros(n*(dm-1),m) zeros(n*(dm-1),2*n) zeros(n*(dm-1),l) zeros(n*(dm-1),5*n) zeros(n*(dm-1),(dM-dm)*n);...
    m1*(A10*(Vp1*X11*Vp1'+Vpp1*X21*Vpp1')+B10*Y1*C1) m1*(Ad10*W1) zeros(n,(dM-1)*n) -Vp2*X12*Vp2'-Vpp2*X22*Vpp2' zeros(n,m) zeros(n,2*n) zeros(n,l) zeros(n,5*n) zeros(n,(dM-dm)*n);...
    R2*(Y1*C1) zeros(m,n) zeros(m,(dM-1)*n) zeros(m,n) -gama*eye(m) zeros(m,2*n) zeros(m,l) zeros(m,5*n) zeros(m,(dM-dm)*n);...
    Vp1*X11*Vp1'+Vpp1*X21*Vpp1' zeros(n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) -W1 zeros(n) zeros(n,l) zeros(n,5*n) zeros(n,(dM-dm)*n);
    sqrt(dM-1)*(Vp1*X11*Vp1'+Vpp1*X21*Vpp1') zeros(n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n) -W2 zeros(n,l) zeros(n,5*n) zeros(n,(dM-dm)*n);...
    R1*(C1*(Vp1*X11*Vp1'+Vpp1*X21*Vpp1')) zeros(l,n) zeros(l,(dM-1)*n) zeros(l,n) zeros(l,m) zeros(l,2*n) -gama*eye(l) zeros(l,5*n) zeros(l,(dM-dm)*n);...
    m2*E11bar*(Vp1*X11*Vp1'+Vpp1*X21*Vpp1') zeros(n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,2*n) zeros(n,l) -Vp2*X12*Vp2'-Vpp2*X22*Vpp2' zeros(n,4*n) zeros(n,(dM-dm)*n);...
    m2*G11bar*Y1*C1 zeros(n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,2*n) zeros(n,l) zeros(n) -Vp2*X12*Vp2'-Vpp2*X22*Vpp2' zeros(n,3*n) zeros(n,(dM-dm)*n);...
    m2*Alpha*F1*(Vp1*X11*Vp1'+Vpp1*X21*Vpp1') zeros(n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,2*n) zeros(n,l) zeros(n,2*n) -Landa2*eye(n) zeros(n,2*n) zeros(n,(dM-dm)*n);...
    zeros(n) m2*F11bar*W1 zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,2*n) zeros(n,l) zeros(n,3*n) -Vp2*X12*Vp2'-Vpp2*X22*Vpp2' zeros(n) zeros(n,(dM-dm)*n);...
    zeros(n) m2*Beta*H1*W1 zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,2*n) zeros(n,l) zeros(n,4*n) -Landa2*eye(n) zeros(n,(dM-dm)*n);...
    zeros((dM-dm)*n,2*n) blkdiag(W2,W2) zeros((dM-dm)*n,(dm-1)*n) zeros((dM-dm)*n,n) zeros((dM-dm)*n,m) zeros((dM-dm)*n,2*n) zeros((dM-dm)*n,l) zeros((dM-dm)*n,5*n) blkdiag(-W1,-W1)]<=0];

%LMI#1      j=2;s=1;
C=C+[[-Vp2*X12*Vp2'-Vpp2*X22*Vpp2' zeros(n) zeros(n,(dM-1)*n) m1*(A20*(Vp2*X12*Vp2'+Vpp2*X22*Vpp2')+B20*Y2*C2)' (Y2*C2)'*R2 Vp2*X12*Vp2'+Vpp2*X22*Vpp2' sqrt(dM-1)*(Vp2*X12*Vp2'+Vpp2*X22*Vpp2') (C2*(Vp2*X12*Vp2'+Vpp2*X22*Vpp2'))'*R1 (m2*E12bar*(Vp2*X12*Vp2'+Vpp2*X22*Vpp2'))' (m2*G12bar*Y2*C2)' (m2*Alpha*F2*(Vp2*X12*Vp2'+Vpp2*X22*Vpp2'))' zeros(n,2*n) zeros(n,(dM-dm)*n);...
    zeros(n) -W1 zeros(n,(dM-1)*n) m1*(Ad20*W1)' zeros(n,m) zeros(n,2*n) zeros(n,l) zeros(n,3*n) (m2*F12bar*W1)' (m2*Beta*H2*W1)' zeros(n,(dM-dm)*n);...
    zeros(n*(dM-dm),2*n) blkdiag(-W2,-W2) zeros(n*(dM-dm),n*(dm-1)) zeros(n*(dM-dm),n) zeros(n*(dM-dm),m) zeros(n*(dM-dm),2*n) zeros(n*(dM-dm),l) zeros(n*(dM-dm),5*n) blkdiag(W2,W2);...
    zeros(n*(dm-1),2*n) zeros(n*(dm-1),n*(dM-dm)) -W2 zeros(n*(dm-1),n) zeros(n*(dm-1),m) zeros(n*(dm-1),2*n) zeros(n*(dm-1),l) zeros(n*(dm-1),5*n) zeros(n*(dm-1),(dM-dm)*n);...
    m1*(A20*(Vp2*X12*Vp2'+Vpp2*X22*Vpp2')+B20*Y2*C2) m1*(Ad20*W1) zeros(n,(dM-1)*n) -Vp1*X11*Vp1'-Vpp1*X21*Vpp1' zeros(n,m) zeros(n,2*n) zeros(n,l) zeros(n,5*n) zeros(n,(dM-dm)*n);...
    R2*(Y2*C2) zeros(m,n) zeros(m,(dM-1)*n) zeros(m,n) -gama*eye(m) zeros(m,2*n) zeros(m,l) zeros(m,5*n) zeros(m,(dM-dm)*n);...
    Vp2*X12*Vp2'+Vpp2*X22*Vpp2' zeros(n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) -W1 zeros(n) zeros(n,l) zeros(n,5*n) zeros(n,(dM-dm)*n);
    sqrt(dM-1)*(Vp2*X12*Vp2'+Vpp2*X22*Vpp2') zeros(n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n) -W2 zeros(n,l) zeros(n,5*n) zeros(n,(dM-dm)*n);...
    R1*(C2*(Vp2*X12*Vp2'+Vpp2*X22*Vpp2')) zeros(l,n) zeros(l,(dM-1)*n) zeros(l,n) zeros(l,m) zeros(l,2*n) -gama*eye(l) zeros(l,5*n) zeros(l,(dM-dm)*n);...
    m2*E12bar*(Vp2*X12*Vp2'+Vpp2*X22*Vpp2') zeros(n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,2*n) zeros(n,l) -Vp1*X11*Vp1'-Vpp1*X21*Vpp1' zeros(n,4*n) zeros(n,(dM-dm)*n);...
    m2*G12bar*Y2*C2 zeros(n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,2*n) zeros(n,l) zeros(n) -Vp1*X11*Vp1'-Vpp1*X21*Vpp1' zeros(n,3*n) zeros(n,(dM-dm)*n);...
    m2*Alpha*F2*(Vp2*X12*Vp2'+Vpp2*X22*Vpp2') zeros(n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,2*n) zeros(n,l) zeros(n,2*n) -Landa1*eye(n) zeros(n,2*n) zeros(n,(dM-dm)*n);...
    zeros(n) m2*F12bar*W1 zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,2*n) zeros(n,l) zeros(n,3*n) -Vp1*X11*Vp1'-Vpp1*X21*Vpp1' zeros(n) zeros(n,(dM-dm)*n);...
    zeros(n) m2*Beta*H2*W1 zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,2*n) zeros(n,l) zeros(n,4*n) -Landa1*eye(n) zeros(n,(dM-dm)*n);...
    zeros((dM-dm)*n,2*n) blkdiag(W2,W2) zeros((dM-dm)*n,(dm-1)*n) zeros((dM-dm)*n,n) zeros((dM-dm)*n,m) zeros((dM-dm)*n,2*n) zeros((dM-dm)*n,l) zeros((dM-dm)*n,5*n) blkdiag(-W1,-W1)]<=0];

%LMI#1      j=2;s=2;
C=C+[[-Vp2*X12*Vp2'-Vpp2*X22*Vpp2' zeros(n) zeros(n,(dM-1)*n) m1*(A20*(Vp2*X12*Vp2'+Vpp2*X22*Vpp2')+B20*Y2*C2)' (Y2*C2)'*R2 Vp2*X12*Vp2'+Vpp2*X22*Vpp2' sqrt(dM-1)*(Vp2*X12*Vp2'+Vpp2*X22*Vpp2') (C2*(Vp2*X12*Vp2'+Vpp2*X22*Vpp2'))'*R1 (m2*E12bar*(Vp2*X12*Vp2'+Vpp2*X22*Vpp2'))' (m2*G12bar*Y2*C2)' (m2*Alpha*F2*(Vp2*X12*Vp2'+Vpp2*X22*Vpp2'))' zeros(n,2*n) zeros(n,(dM-dm)*n);...
    zeros(n) -W1 zeros(n,(dM-1)*n) m1*(Ad20*W1)' zeros(n,m) zeros(n,2*n) zeros(n,l) zeros(n,3*n) (m2*F12bar*W1)' (m2*Beta*H2*W1)' zeros(n,(dM-dm)*n);...
    zeros(n*(dM-dm),2*n) blkdiag(-W2,-W2) zeros(n*(dM-dm),n*(dm-1)) zeros(n*(dM-dm),n) zeros(n*(dM-dm),m) zeros(n*(dM-dm),2*n) zeros(n*(dM-dm),l) zeros(n*(dM-dm),5*n) blkdiag(W2,W2);...
    zeros(n*(dm-1),2*n) zeros(n*(dm-1),n*(dM-dm)) -W2 zeros(n*(dm-1),n) zeros(n*(dm-1),m) zeros(n*(dm-1),2*n) zeros(n*(dm-1),l) zeros(n*(dm-1),5*n) zeros(n*(dm-1),(dM-dm)*n);...
    m1*(A20*(Vp2*X12*Vp2'+Vpp2*X22*Vpp2')+B20*Y2*C2) m1*(Ad20*W1) zeros(n,(dM-1)*n) -Vp2*X12*Vp2'-Vpp2*X22*Vpp2' zeros(n,m) zeros(n,2*n) zeros(n,l) zeros(n,5*n) zeros(n,(dM-dm)*n);...
    R2*(Y2*C2) zeros(m,n) zeros(m,(dM-1)*n) zeros(m,n) -gama*eye(m) zeros(m,2*n) zeros(m,l) zeros(m,5*n) zeros(m,(dM-dm)*n);...
    Vp2*X12*Vp2'+Vpp2*X22*Vpp2' zeros(n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) -W1 zeros(n) zeros(n,l) zeros(n,5*n) zeros(n,(dM-dm)*n);
    sqrt(dM-1)*(Vp2*X12*Vp2'+Vpp2*X22*Vpp2') zeros(n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n) -W2 zeros(n,l) zeros(n,5*n) zeros(n,(dM-dm)*n);...
    R1*(C2*(Vp2*X12*Vp2'+Vpp2*X22*Vpp2')) zeros(l,n) zeros(l,(dM-1)*n) zeros(l,n) zeros(l,m) zeros(l,2*n) -gama*eye(l) zeros(l,5*n) zeros(l,(dM-dm)*n);...
    m2*E12bar*(Vp2*X12*Vp2'+Vpp2*X22*Vpp2') zeros(n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,2*n) zeros(n,l) -Vp2*X12*Vp2'-Vpp2*X22*Vpp2' zeros(n,4*n) zeros(n,(dM-dm)*n);...
    m2*G12bar*Y2*C2 zeros(n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,2*n) zeros(n,l) zeros(n) -Vp2*X12*Vp2'-Vpp2*X22*Vpp2' zeros(n,3*n) zeros(n,(dM-dm)*n);...
    m2*Alpha*F2*(Vp2*X12*Vp2'+Vpp2*X22*Vpp2') zeros(n) zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,2*n) zeros(n,l) zeros(n,2*n) -Landa2*eye(n) zeros(n,2*n) zeros(n,(dM-dm)*n);...
    zeros(n) m2*F12bar*W1 zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,2*n) zeros(n,l) zeros(n,3*n) -Vp2*X12*Vp2'-Vpp2*X22*Vpp2' zeros(n) zeros(n,(dM-dm)*n);...
    zeros(n) m2*Beta*H2*W1 zeros(n,(dM-1)*n) zeros(n) zeros(n,m) zeros(n,2*n) zeros(n,l) zeros(n,4*n) -Landa2*eye(n) zeros(n,(dM-dm)*n);...
    zeros((dM-dm)*n,2*n) blkdiag(W2,W2) zeros((dM-dm)*n,(dm-1)*n) zeros((dM-dm)*n,n) zeros((dM-dm)*n,m) zeros((dM-dm)*n,2*n) zeros((dM-dm)*n,l) zeros((dM-dm)*n,5*n) blkdiag(-W1,-W1)]<=0];

%LMI#2  j=1,j=2;
C=C+[(Vp1*X11*Vp1'+Vpp1*X21*Vpp1')-((zita/eps1)*eye(n))>=0,(Vp2*X12*Vp2'+Vpp2*X22*Vpp2')-((zita/eps1)*eye(n))>=0,W1-(((zita*dM)/eps2)*eye(n))>=0,W2-(((zita*dM*(dM-1))/(2-2*eps1-2*eps2))*eye(n))>=0];                     %New LMI

%LMI#3  s=1,s=2;
C=C+[Vp1*X11*Vp1'+Vpp1*X21*Vpp1'-Landa1*eye(n)>=0, Vp2*X12*Vp2'+Vpp2*X22*Vpp2'-Landa2*eye(n)>=0];

if Cu==1
    
    %input constraint  j=1;
    C=C+[[G1 Y1*C1;...
        (Y1*C1)' Vp1*X11*Vp1'+Vpp1*X21*Vpp1']>=0];
    
    %input constraint  j=2;
    C=C+[[G1 Y2*C2;...
        (Y2*C2)' Vp2*X12*Vp2'+Vpp2*X22*Vpp2']>=0];
    
    C=C+[[G1-blkdiag(u1max^2)]<=0];
    
end

if Cy==1
    
    %output constraint  j=1;
    C=C+[[G2 C1*(Vp1*X11*Vp1'+Vpp1*X21*Vpp1');...
        (Vp1*X11*Vp1'+Vpp1*X21*Vpp1')*C1' (Vp1*X11*Vp1'+Vpp1*X21*Vpp1')]>=0];
    
    %output constraint  j=2;
    C=C+[[G2 C2*(Vp2*X12*Vp2'+Vpp2*X22*Vpp2');...
        (Vp2*X12*Vp2'+Vpp2*X22*Vpp2')'*C2' (Vp2*X12*Vp2'+Vpp2*X22*Vpp2')]>=0];
    
    C=C+[[G2-blkdiag(y1max^2)]<=0];
    
end

o=gama;
op=sdpsettings('solver','lmilab,*');
diag=optimize(C,gama,op)

Y1v=value(Y1);Y2v=value(Y2);
%S1v=value(S1);S2v=value(S2);
X11v=value(X11);X12v=value(X12);
X21v=value(X21);X22v=value(X22);
W1v=value(W1);W2v=value(W2);
Landa1v=value(Landa1);Landa2v=value(Landa2);
gamav=value(gama);

S1v=Vp1*X11v*Vp1'+Vpp1*X21v*Vpp1';
S2v=Vp2*X12v*Vp2'+Vpp2*X22v*Vpp2';

% X11=V1'*S1v*V1;X11=X11(1,1);
% X12=V2'*S2v*V2;X12=X12(1,1);

K1=Y1v*U1*C10*inv(X11v)*inv(C10)*U1';
K2=Y2v*U2*C20*inv(X12v)*inv(C20)*U2';

for i=T0:Tf
    
    delta=el*sin(i);
    z=mod(i,2);
    if (z>0)
        xSOF(:,i)=(A10+delta*E11)*xSOF(:,i-1)+(Ad10+delta*F11)*xSOF(:,i-1-d(i))+(B10+delta*G11)*K1*C1*xSOF(:,i-1)+[0.01*sin(xSOF(1,i-1));0]+[0.002*sin(xSOF(1,i-1-d(i)));0];
        ySOF(:,i-1)=C1*xSOF(:,i-1);
        uSOF(:,i-1)=K1*ySOF(:,i-1);
        nSOF(i) =norm(xSOF(:,i));
        sSOF(i)=1;
        VSOF(i-1)=xSOF(:,i-1)'*gamav*inv(S1v)*xSOF(:,i-1);
        for k=-1:-1:-dM
            VSOF(i-1)=VSOF(i-1)+xSOF(:,i-1+k)'*gamav*inv(W1v)*xSOF(:,i-1+k);
        end
        for k=-1:-1:-dM+1
            VSOF(i-1)=VSOF(i-1)+(dM+k)*xSOF(:,i-1+k)'*gamav*inv(W2v)*xSOF(:,i-1+k);
        end
        VSOF(i-1);
    else
        xSOF(:,i)=(A20+delta*E12)*xSOF(:,i-1)+(Ad20+delta*F12)*xSOF(:,i-1-d(i))+(B20+delta*G12)*K2*C2*xSOF(:,i-1)+[0.01*sin(xSOF(1,i-1));0]+[0.002*sin(xSOF(1,i-1-d(i)));0];
        ySOF(:,i-1)=C2*xSOF(:,i-1);
        uSOF(:,i-1)=K2*ySOF(:,i-1);
        nSOF(i) =norm(xSOF(:,i));
        sSOF(i)=2;
        VSOF(i-1)=xSOF(:,i-1)'*gamav*inv(S2v)*xSOF(:,i-1);
        for k=-1:-1:-dM
            VSOF(i-1)=VSOF(i-1)+xSOF(:,i-1+k)'*gamav*inv(W1v)*xSOF(:,i-1+k);
        end
        for k=-1:-1:-dM+1
            VSOF(i-1)=VSOF(i-1)+(dM+k)*xSOF(:,i-1+k)'*gamav*inv(W2v)*xSOF(:,i-1+k);
        end
        VSOF(i-1);
    end
    JSOF=JSOF+ySOF(:,i-1)'*R1*ySOF(:,i-1)+uSOF(:,i-1)'*R2*uSOF(:,i-1);
end

save('Constrained_RMPC_SOF.mat')
CostFunction_ky_InequalQ=JSOF;

uSOF1max=max(uSOF(1,T0-1:Tf-1));
uSOF1min=min(uSOF(1,T0-1:Tf-1));
uSOF1Max=max(abs(uSOF1min),abs(uSOF1max));

ySOF1max=max(ySOF(1,T0:Tf-1));
ySOF1min=min(ySOF(1,T0:Tf-1));
ySOF1Max=max(abs(ySOF1min),abs(ySOF1max));

VSOFMax=max(VSOF(T0-1:Tf-1));

List={};
Result_table = table(dm,dM,CostFunction_ky_InequalQ,uSOF1Max,ySOF1Max,gamav,VSOFMax,'RowNames',List)

t=0:Tf-T0;
figure;stairs(t,xSOF(:,T0-1:Tf-1)','LineWidth',1.5);grid on;xlabel('k (sample)');ylabel('States');legend('x_1','x_2')
figure;stairs(t,sSOF(T0:Tf),'LineWidth',2);grid on;xlabel('k (sample)');ylabel('Switching signal');ylim([0 3])
figure;stairs(t,uSOF(:,T0-1:Tf-1)','LineWidth',1.5);grid on;xlabel('k (sample)');ylabel('Control Signal (u)');
figure;stairs(t,ySOF(:,T0-1:Tf-1)','LineWidth',1.5);grid on;xlabel('k (sample)');ylabel('Output (y)');
figure;plot(1:50,d,'LineWidth',1.5);grid on;xlabel('k (sample)');ylabel('d(k)');
figure;stairs(t,VSOF(T0-1:Tf-1)','LineWidth',1.5);grid on;xlabel('k (sample)');ylabel('V');
%figure;plot(t,nSOF(T0:Tf)','*');grid on;xlabel('k (sample)');ylabel('Norm of state variables');